#!/usr/bin/env python
#-*- coding: utf-8 -*-

import numpy as np
import math
import scipy
import matplotlib.pyplot as plt

import pat.vehicles.fixed_wing.dynamic_model_python_basic as dm
import pat.vehicles.fixed_wing.control_3d as ctl
import pat.vehicles.fixed_wing.plot_3d   as mp
import pat.utils as pu


# loads and displays the dynamic model's parameters
filename = "../config/Rcam.xml"
_dm = dm.DynamicModel(filename)
print _dm.param()

# finds trim conditions
va=70; gamma=pu.rad_of_deg(0.)
Xe, Ue = ctl.get_trim_cst_path(va, gamma, _dm.p, debug=True)

# run simulations
time = np.arange(0., 120., 0.01)
X = np.zeros((time.size,_dm.sv_size))

def run_sim(X0, U, title):
    X[0] = np.array(X0)
    for i in range(1,time.size):
        X[i,:] = _dm.run(time[i] - time[i-1], U[i-1])
    mp.plot_dynamics(time, X, window_title="{:s} ({:s})".format(title,_dm.name))
    
# runs a simulation with throttle steps
U = Ue*np.ones((len(time), _dm.input_nb()))
step = 0.3*scipy.signal.square(0.25*time+math.pi/2.)
#for i in range(0,p_dm.eng_nb): U[:,i] += step
U[:,0] += step
U[:,1] -= step
run_sim(Xe, U, "Differential Throttle steps")

# runs a simulation with one engine
U = Ue*np.ones((len(time), _dm.input_nb()))
U[:,0] *= 2.
U[:,1] *= 0.
run_sim(Xe, U, "One engine")




plt.show()
